"""
brief :
    Editor : CJH
    Date : 2025/1/14
"""
import sys
import os
import time

from pyorbbecsdk import *
import cv2
import numpy as np

import logging

logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(message)s')
logger = logging.getLogger("Orbbec_init")

MIN_DEPTH = 20  # 20mm
MAX_DEPTH = 100000  # 10000mm


class orbbec_mode(object):
    def __init__(self):
        self.Extended_Mode = "Extended Distance Mode"
        self.single_RGB_mode = 0
        self.single_Depth_mode = 1
        self.sync_align_mode = 2


def get_exist_camera():
    camera_list = []
    try:
        device_list = Context().query_devices()  # 相关相机列表
        for i in range(device_list.get_count()):
            device = device_list.get_device_by_index(i)
            camera_list.append(str(device.get_device_info().get_serial_number()))
        return camera_list
    except:
        raise "Orbbec camera not exist or already opened !!!"


class Orbbec(orbbec_mode):
    def __init__(self, device_id: str = None):
        super(Orbbec, self).__init__()
        self.cam_init_mode = None
        self.depth_intrinsic = None
        self.rgb_intrinsic = None
        self.device = self.check_device(device_id)
        self.config = Config()  # 相机设置
        self.pipeline = Pipeline(self.device)  # 相机管道流
        self.width, self.height = None, None

    @staticmethod
    def check_device(device_id: str):
        # 相机选择初始化,获得相机的序列号
        device_list = Context().query_devices()  # 相关相机列表
        if device_id is not None:
            for i in range(device_list.get_count()):
                device = device_list.get_device_by_index(i)
                if str(device.get_device_info().get_serial_number()) == device_id:
                    logger.error(f"Orbbec camera serial Information :{device.get_device_info().get_serial_number()}")
                    return device
            raise "Orbbec camera not exist or already opened !!!"
        else:
            try:
                device = device_list.get_device_by_index(0)
                logger.info(f"Orbbec camera serial Information :{device.get_device_info().get_serial_number()}")
                return device  # 读取返回匹配的相机
            except:
                raise "Orbbec camera not exist or already opened !!!"

    def cam_init(self, ppi=640,
                 cam_init_mode=2,
                 enable_align_sync=True,  # 是否输出对齐帧
                 enable_align_mode="SW",
                 enable_auto_expose=True,
                 enable_auto_white_balance=True,
                 exposure=10000,
                 DEPTH_MODE="Extended Distance Mode"):
        ppi = ppi if ppi == 1280 else 640
        self.cam_init_mode = cam_init_mode
        if cam_init_mode == self.single_RGB_mode:  # RGB mode
            profile_list = self.pipeline.get_stream_profile_list(OBSensorType.COLOR_SENSOR)
            color_profile: VideoStreamProfile = profile_list.get_video_stream_profile(1920, 0, OBFormat.RGB, 30)
            self.config.enable_stream(color_profile)

        elif cam_init_mode == self.single_Depth_mode:  # Depth mode
            profile_list = self.pipeline.get_stream_profile_list(OBSensorType.DEPTH_SENSOR)
            depth_profile: VideoStreamProfile = profile_list.get_video_stream_profile(ppi, 0, OBFormat.Y16, 30)
            self.config.enable_stream(depth_profile)

        else:
            profile_list = self.pipeline.get_stream_profile_list(OBSensorType.COLOR_SENSOR)
            color_profile: VideoStreamProfile = profile_list.get_video_stream_profile(ppi, 0, OBFormat.RGB, 30)
            profile_list = self.pipeline.get_stream_profile_list(OBSensorType.DEPTH_SENSOR)
            depth_profile: VideoStreamProfile = profile_list.get_video_stream_profile(ppi, 0, OBFormat.Y16, 30)
            self.config.enable_stream(color_profile)
            self.config.enable_stream(depth_profile)

        # 设置相机自动曝光
        if enable_auto_expose:
            self.device.set_bool_property(OBPropertyID.OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_auto_expose)
        else:
            self.device.set_int_property(OBPropertyID.OB_PROP_COLOR_EXPOSURE_INT, exposure)
        # 设置相机白平衡
        self.device.set_bool_property(OBPropertyID.OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, enable_auto_white_balance)
        self.device.set_bool_property(OBPropertyID.OB_PROP_LASER_BOOL, True)  # 将激光默认开启
        # 设置相机模式
        try:
            self.device.set_depth_work_mode(DEPTH_MODE)
        # self.device.(OBPropertyID.OB_STRUCT_CURRENT_DEPTH_ALG_MODE, "Extended Distance Mode")
        # self.device.set_bool_property(OBPropertyID.OB_PROP_DEPTH_SOFT_FILTER_BOOL, True)
        # self.device.set_bool_property(OBPropertyID.OB_PROP_LASER_BOOL, True)
        except:
            pass
        # 开启相机同步
        if enable_align_sync and cam_init_mode == self.sync_align_mode:
            if enable_align_mode == "SW":
                self.config.set_align_mode(OBAlignMode.SW_MODE)
            else:
                self.config.set_align_mode(OBAlignMode.HW_MODE)
            try:
                self.pipeline.enable_frame_sync()
            except Exception as e:
                logger.error(e)

        # 开启相机视频流
        self.pipeline.start(self.config)
        self.depth_intrinsic = self.pipeline.get_camera_param().depth_intrinsic
        self.rgb_intrinsic = self.pipeline.get_camera_param().rgb_intrinsic
        # self.width, self.height = self.depth_intrinsic.width, self.depth_intrinsic.height
        logger.warning("cam profile :RGB_intrin:{},Depth_intrin:{},".format(
            self.rgb_intrinsic,
            self.depth_intrinsic))

    def cam_run(self, delay=100):
        frames: FrameSet = self.pipeline.wait_for_frames(delay)
        if Context().query_devices().get_count() == 0:
            sys.exit()
        if frames is None:
            return None, None, None
        if self.cam_init_mode == self.single_RGB_mode:
            if frames.get_color_frame() is None:
                return None, None, None

            color_frame = frames.get_color_frame()
            color_image = np.resize(np.asanyarray(color_frame.get_data()),
                                    (color_frame.get_height(), color_frame.get_width(), 3))
            color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)

            return color_image.astype(np.uint8), None, None

        elif self.cam_init_mode == self.single_Depth_mode:
            # 深度信息转换成对齐格式
            if frames.get_depth_frame() is None:
                return None, None, None
            depth_frame = frames.get_depth_frame()
            depth_data = np.frombuffer(depth_frame.get_data(), dtype=np.uint16)
            depth_data = depth_data.reshape((depth_frame.get_height(), depth_frame.get_width())).astype(
                np.float32) * depth_frame.get_depth_scale()
            depth_data = np.where((depth_data > MIN_DEPTH) & (depth_data < MAX_DEPTH), depth_data, 0)  # 深度信息过滤
            # depth_data = depth_data.astype(np.uint16)
            # 对深度信息归一化呈现在图上
            depth_image = cv2.normalize(depth_data, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U)
            depth_image = cv2.applyColorMap(depth_image, cv2.COLORMAP_JET)
            return None, depth_image.astype(np.uint8), depth_data
        else:
            if frames.get_color_frame() is None or frames.get_depth_frame() is None:
                return None, None, None

            color_frame = frames.get_color_frame()
            depth_frame = frames.get_depth_frame()
            # 将color转换成BGR格式的cv输出
            color_image = np.resize(np.asanyarray(color_frame.get_data()),
                                    (color_frame.get_height(), color_frame.get_width(), 3))
            color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)

            # 深度信息转换成对齐格式
            depth_data = np.frombuffer(depth_frame.get_data(), dtype=np.uint16)
            depth_data = depth_data.reshape((depth_frame.get_height(), depth_frame.get_width())).astype(
                np.float32) * depth_frame.get_depth_scale()
            depth_data = np.where((depth_data > MIN_DEPTH) & (depth_data < MAX_DEPTH), depth_data, 0)  # 深度信息过滤
            # depth_data = depth_data.astype(np.uint16)
            # 对深度信息归一化呈现在图上
            depth_image = cv2.normalize(depth_data, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U)
            depth_image = cv2.applyColorMap(depth_image, cv2.COLORMAP_JET)

            return color_image.astype(np.uint8), depth_image.astype(np.uint8), depth_data


window_open = True
FRONT_INDEX = 0
RIGHT_INDEX = 1
LEFT_INDEX = 1

import json


def mouse_callback(event, x, y, flags, param):
    global window_open
    # 检测左键点击事件
    if event == cv2.EVENT_LBUTTONDOWN or event == cv2.EVENT_RBUTTONDOWN:
        window_open = False  # 设置标志，退出循环


def main(argv):
    start_time = time.time()
    dict = {"camera": {'mid': "", "right": "", 'left': ""}}
    orbbec = Orbbec(device_id=argv[3])
    orbbec.cam_init(1280, cam_init_mode=orbbec.sync_align_mode, enable_align_sync=True,  # 是否输出对齐帧
                    enable_align_mode="SW",
                    enable_auto_expose=True,
                    enable_auto_white_balance=True)
    if os.path.exists(f"{argv[1]}/cam_SN_list.json"):
        with open(file=f"{argv[1]}/cam_SN_list.json", mode="r") as file:
            dict = json.loads(file.read())

    while time.time() - start_time < 5:
        color_image, _, _ = orbbec.cam_run()
        if color_image is None:
            continue
        cv2.putText(color_image, f"last time: {5-round(time.time() - start_time)}s", (50, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255), 2)
        cv2.imshow(str(argv[-1]), color_image)
        cv2.setMouseCallback(str(argv[-1]), mouse_callback)
        if cv2.waitKey(1) > 0 or not window_open:
            cv2.destroyAllWindows()
            break
    dict["camera"][argv[2]] = argv[3]
    with open(
            file=f"{argv[1]}/cam_SN_list.json",
            mode="w",
            encoding="utf-8") as file:
        file.write(json.dumps(dict))


if __name__ == "__main__":
    argv = [sys.argv[0], "D:\eyedetector\data\camera_inter_data", "mid", "AYZDC430016"]
    main(argv)

# 2025-03-12 14:17:47,934 - cam profile :RGB_intrin:<OBCameraIntrinsic fx=1267.614746 fy=1267.594482 cx=964.284119 cy=546.878784 width=1920 height=1080>,Depth_intrin:<OBCameraIntrinsic fx=958.487915 fy=958.487915 cx=644.125488 cy=402.027710 width=1280 height=800>,
# 2025-03-12 14:18:11,904 - cam profile :RGB_intrin:<OBCameraIntrinsic fx=845.076538 fy=845.062988 cx=642.856079 cy=364.585876 width=1280 height=720>,Depth_intrin:<OBCameraIntrinsic fx=845.076538 fy=845.062988 cx=642.856079 cy=364.585876 width=1280 height=720>,
